"""
GomerX 底盘控制模块
用于控制机器人底盘的移动和转向
"""

# 导入底盘配置参数
from config.settings import CHASSIS_CONFIG

class Chassis:
    """GomerX 底盘控制类
    
    负责控制机器人的移动、转向和循迹功能
    包含PID控制器实现，用于精确的线路跟踪
    """
    
    def __init__(self):
        """初始化底盘控制器
        
        从配置文件加载底盘参数，包括速度、转向速度和PID控制器参数
        """
        # 从配置文件加载基本运动参数
        self.speed = CHASSIS_CONFIG["speed"]  # 默认前进/后退速度(米/秒)
        self.turn_speed = CHASSIS_CONFIG["turn_speed"]  # 默认转向速度(弧度/秒)
        
        # 底盘连接状态，初始为未连接
        self.is_connected = False
        
        # PID控制器参数配置
        # PID控制器是一种常用的控制算法，用于根据误差调整控制量
        self.kp = CHASSIS_CONFIG["line_follow"]["kp"]  # 比例系数，控制对当前误差的响应强度
        self.ki = CHASSIS_CONFIG["line_follow"]["ki"]  # 积分系数，控制对累积误差的响应强度
        self.kd = CHASSIS_CONFIG["line_follow"]["kd"]  # 微分系数，控制对误差变化率的响应强度
        self.target_offset = CHASSIS_CONFIG["line_follow"]["target_offset"]  # 目标偏移量，通常为0表示中心位置
        
        # PID控制器状态变量
        self.last_error = 0  # 上一次的误差值，用于计算微分项
        self.integral = 0  # 误差累积值，用于计算积分项
        
    def connect(self):
        """连接到底盘
        
        初始化与底盘硬件的通信连接
        
        Returns:
            bool: 连接是否成功
        """
        try:
            # 这里应该是连接到底盘的实际代码
            # 例如：初始化串口通信、网络连接或硬件接口
            # 由于没有真实的SDK，这里只是模拟连接
            self.is_connected = True  # 更新连接状态
            return True  # 返回连接成功
        except Exception as e:
            # 捕获连接过程中的异常
            print(f"连接底盘失败: {e}")  # 输出错误信息
            return False  # 返回连接失败
    
    def disconnect(self):
        """断开与底盘的连接
        
        释放底盘硬件资源，关闭通信连接
        """
        # 检查是否已连接
        if self.is_connected:
            # 这里应该是断开连接的实际代码
            # 例如：关闭串口、断开网络连接或释放硬件接口
            self.is_connected = False  # 更新连接状态
    
    def move_forward(self, distance=None, speed=None):
        """
        控制底盘前进
        
        Args:
            distance: 前进距离(米)，如果为None则持续前进直到收到停止命令
            speed: 前进速度(米/秒)，如果为None则使用默认速度
            
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的速度：优先使用传入参数，否则使用默认速度
        move_speed = speed if speed is not None else self.speed
        
        try:
            # 这里应该是控制底盘前进的实际代码
            # 例如：向底盘发送前进指令，包含速度和距离参数
            print(f"底盘前进，速度: {move_speed} m/s，距离: {distance} m")  # 调试输出
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"控制底盘前进失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def move_backward(self, distance=None, speed=None):
        """
        控制底盘后退
        
        Args:
            distance: 后退距离(米)，如果为None则持续后退直到收到停止命令
            speed: 后退速度(米/秒)，如果为None则使用默认速度
            
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的速度：优先使用传入参数，否则使用默认速度
        move_speed = speed if speed is not None else self.speed
        
        try:
            # 这里应该是控制底盘后退的实际代码
            # 例如：向底盘发送后退指令，包含速度和距离参数
            print(f"底盘后退，速度: {move_speed} m/s，距离: {distance} m")  # 调试输出
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"控制底盘后退失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def turn_left(self, angle=None, speed=None):
        """
        控制底盘左转
        
        Args:
            angle: 转向角度(弧度)，如果为None则持续左转直到收到停止命令
            speed: 转向速度(弧度/秒)，如果为None则使用默认转向速度
            
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的转向速度：优先使用传入参数，否则使用默认转向速度
        turn_speed = speed if speed is not None else self.turn_speed
        
        try:
            # 这里应该是控制底盘左转的实际代码
            # 例如：向底盘发送左转指令，包含角度和速度参数
            print(f"底盘左转，速度: {turn_speed} rad/s，角度: {angle} rad")  # 调试输出
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"控制底盘左转失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def turn_right(self, angle=None, speed=None):
        """
        控制底盘右转
        
        Args:
            angle: 转向角度(弧度)，如果为None则持续右转直到收到停止命令
            speed: 转向速度(弧度/秒)，如果为None则使用默认转向速度
            
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定实际使用的转向速度：优先使用传入参数，否则使用默认转向速度
        turn_speed = speed if speed is not None else self.turn_speed
        
        try:
            # 这里应该是控制底盘右转的实际代码
            # 例如：向底盘发送右转指令，包含角度和速度参数
            print(f"底盘右转，速度: {turn_speed} rad/s，角度: {angle} rad")  # 调试输出
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"控制底盘右转失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def stop(self):
        """停止底盘移动
        
        立即停止所有运动，包括前进、后退和转向
        
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        try:
            # 这里应该是停止底盘的实际代码
            # 例如：向底盘发送停止指令，立即停止所有电机
            print("底盘停止")  # 调试输出
            return True  # 返回成功
        except Exception as e:
            # 捕获控制过程中的异常
            print(f"停止底盘失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def calculate_pid_control(self, offset):
        """
        基于PID控制器计算转向控制量
        
        PID控制器是一种常用的控制算法，通过比例(P)、积分(I)、微分(D)三项
        来计算控制量，实现对目标值的精确跟踪
        
        Args:
            offset: 线条相对于图像中心的偏移量，范围[-1, 1]
                    -1表示线条在最左侧，1表示线条在最右侧，0表示居中
            
        Returns:
            float: 转向控制量，范围[-1, 1]
                   正值表示右转，负值表示左转，0表示直行
        """
        # 计算当前误差：目标偏移量减去实际偏移量
        error = self.target_offset - offset
        
        # 计算PID各项
        # P项（比例项）：与当前误差成正比，提供基本的控制响应
        p_term = self.kp * error
        
        # I项（积分项）：与误差累积成正比，用于消除稳态误差
        self.integral += error  # 累积误差
        i_term = self.ki * self.integral
        
        # D项（微分项）：与误差变化率成正比，用于减少超调和振荡
        d_term = self.kd * (error - self.last_error)
        
        # 更新上次误差，用于下次计算微分项
        self.last_error = error
        
        # 计算总控制量：P、I、D三项之和
        control = p_term + i_term + d_term
        
        # 限制控制量范围在[-1, 1]之间，防止控制量过大
        control = max(-1.0, min(1.0, control))
        
        # 返回计算得到的控制量
        return control
    
    def follow_line(self, offset):
        """
        基于线条偏移量控制底盘循迹
        
        使用PID控制器计算转向控制量，并根据控制量调整底盘运动
        
        Args:
            offset: 线条相对于图像中心的偏移量，范围[-1, 1]
                    -1表示线条在最左侧，1表示线条在最右侧，0表示居中
            
        Returns:
            bool: 控制是否成功
        """
        # 检查底盘是否已连接
        if not self.is_connected:
            print("底盘未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 使用PID控制器计算转向控制量
        control = self.calculate_pid_control(offset)
        
        # 根据控制量调整底盘运动
        if abs(control) < 0.1:  # 控制量接近0，表示线条居中，直线行驶
            return self.move_forward()  # 直线前进
        elif control > 0:  # 控制量为正，表示线条偏左，需要右转
            turn_angle = control * 0.5  # 将控制量映射到转向角度，0.5是比例系数
            return self.turn_right(angle=turn_angle)  # 右转指定角度
        else:  # 控制量为负，表示线条偏右，需要左转
            turn_angle = -control * 0.5  # 将控制量映射到转向角度，取绝对值
            return self.turn_left(angle=turn_angle)  # 左转指定角度
    
    def reset_pid(self):
        """重置PID控制器状态
        
        清除误差累积和上次误差值，通常在开始新的循迹任务前调用
        """
        self.last_error = 0  # 重置上次误差
        self.integral = 0  # 重置误差累积